﻿using UnityEngine;
using System.Collections.Generic;
namespace Room
{
    public class RCMoveNavigation : RCMoveBase
    {
        public RCMoveNavigation(RoomCharactor owner)
            :base(owner)
        {
            type = MoveType.Navigation;
        }

        NavMeshAgent _agent;
        NavMeshAgent agent
        {
            get
            {
                if (_agent == null)
                {
                    _agent = owner.gameObject.AddMissingComponent<NavMeshAgent>();
                    _agent.angularSpeed = rotateSpeed;
                    _agent.acceleration = 100;
                    _agent.radius = 0.0f;
                    _agent.speed = moveSpeed;
                    _agent.autoRepath = false;
                    _agent.obstacleAvoidanceType = ObstacleAvoidanceType.LowQualityObstacleAvoidance;
                }
                return _agent;
            }
        }

        private float moveSpeed = 5f;
        private float rotateSpeed = 360f*2;

        private bool running = false;
        private bool first = false;
        private Queue<Vector3> corners = new Queue<Vector3>();

        private bool _usingNavAgent = true;
        public bool usingNavAgent
        {
            set
            {
                if(_usingNavAgent!=value)
                {
                    _usingNavAgent = value;
                    agent.enabled = value;
                }
            }
            get
            {
                return _usingNavAgent;
            }
        }

        public override void DoStart()
        {
            first = true;
        }

        public override void DoResume()
        {
            running = true;
        }

        public override void DoPause()
        {
            running = false;
            corners.Clear();

            //if (owner.type == CharactorType.SelfPlayer)
            //    Debug.Log("SelfPlayer DoPause");
        }

        public override void Tick()
        {
            if (first)
            {
                NavMeshHit hit;
                if (NavMesh.SamplePosition(owner.transform.position, out hit, 100f, NavMesh.AllAreas))
                {
                    first = false;
                    owner.transform.position = hit.position;
                }
            }
            if (corners.Count == 0)
                return;
            
            Vector3 target = corners.Peek();

            Vector3 targetDir = target - owner.transform.position;
            float maxDistance = targetDir.magnitude;
            targetDir.y = 0;
            targetDir.Normalize();
            Vector3 curDir = owner.transform.forward;
            Vector3 dstDir = Vector3.RotateTowards(curDir, targetDir, rotateSpeed * Time.deltaTime*Mathf.Deg2Rad, 1f);

            if (dstDir != Vector3.zero)
            {
                owner.transform.forward = dstDir;
            }

            float distance = Vector3.Dot(targetDir, dstDir) ;
            if (distance < 0)
                distance = distance * 0.3f + 0.3f;
            else
                distance = distance * 0.7f + 0.3f;
            //Debug.Log(distance);
            distance = distance * moveSpeed * Time.deltaTime;
            distance = Mathf.Min(distance, maxDistance);
            distance = Mathf.Max(distance, 0);
            Debug.DrawLine(agent.transform.position, agent.transform.position + distance * targetDir);
            if (usingNavAgent)
            {
                if(agent.isOnNavMesh)
                {
                    agent.Move(distance * targetDir);
                }
                else
                {
                    SamplePosition(owner.transform.position);
                    return;
                }
                
            }
            else
            {
                owner.transform.position+=(distance * targetDir);
            }

            if ((owner.transform.position - target).magnitude < 0.1f)
            {
                corners.Dequeue();
            }
        }

        public override void MoveTo(Vector3 to)
        {
            //CalculatePath(to);
            corners.Clear();
            corners.Enqueue(to);
            //if (owner.type == CharactorType.SelfPlayer)
            //    Debug.Log("SelfPlayer StopTo:" + to);
        }

        public override void StopTo(Vector3 to)
        {
            
            if (running)
                CalculatePath(to);
            else
                SamplePosition(to);
            //if (owner.type == CharactorType.SelfPlayer)
            //    Debug.Log("SelfPlayer StopTo:" + to);
            
        }

        public override void LerpPosition(Vector3 to)
        {
            if(SamplePosition(to,out to))
            {
                if (corners.Count > 0)
                {
                    var dis = float.MaxValue;

                    var pos0 = new Vector2(to.x, to.z);
                    int index = 0;
                    Vector3 nearPos = Vector3.zero;
                    Queue<Vector3> temp = new Queue<Vector3>();
                    while (corners.Count > 0)
                    {
                        var posSrc = corners.Dequeue();
                        var pos1 = new Vector2(posSrc.x, posSrc.z);
                        if (dis > Vector2.Distance(pos0, pos1))
                        {
                            nearPos = posSrc;
                        }
                        index++;
                        temp.Enqueue(posSrc);
                    }

                    while (temp.Count > 0)
                    {
                        var posT = temp.Dequeue();
                        if (posT == nearPos)
                        {
                            var newPos = (posT + to) / 2f;

                            if (SamplePosition(newPos, out newPos))
                            {
                                posT = newPos;
                            }
                        }
                        corners.Enqueue(posT);
                    }
                }
                else
                {
                    corners.Enqueue(to);
                }


                Debug.Log("[Lerp pos] to:" + to);
            }
        }

        public override void StopMove()
        {
            base.StopMove();
            corners.Clear();
        }

        public override void SetSpeed(float speed)
        {
            agent.speed = speed;
            moveSpeed = speed;
        }

        public override bool IsMoving()
        {
            return corners.Count>0;
        }
        
        private void CalculatePath(Vector3 target)
        {
            
            if(usingNavAgent)
            {
                NavMeshPath path = new NavMeshPath();
                if (NavMesh.CalculatePath(owner.transform.position, target, NavMesh.AllAreas, path))
                {
                    corners.Clear();
                    for (int i = 0; i < path.corners.Length; i++)
                    {
                        corners.Enqueue(path.corners[i]);
                    }
                }
                while (corners.Count > 0 && (owner.transform.position - corners.Peek()).magnitude < 0.1f)
                {
                    corners.Dequeue();
                }
            }
            else
            {
                if(target!=owner.transform.position)
                    corners.Enqueue(target);
            }
            
        }

        private void SamplePosition(Vector3 target)
        {
            NavMeshHit hit;
            if (NavMesh.SamplePosition(target, out hit, 10f, NavMesh.AllAreas))
            {
                corners.Clear();
                owner.transform.position = hit.position;
            }
        }

        public static bool SamplePosition(Vector3 src,out Vector3 target)
        {
            target = src;
            NavMeshHit hit;
            bool canGo = NavMesh.SamplePosition(src, out hit, 100f, NavMesh.AllAreas);
            if(canGo)
            {
                target = hit.position;
            }

            return canGo;
        }

        public override void ResetPosition()
        {
            NavMeshHit hit;
            if (NavMesh.SamplePosition(owner.transform.position, out hit, 10, NavMesh.AllAreas))
            {
                owner.transform.position = hit.position;
            }
        }
    }
}

